package ar.edu.untref.robotica.simulador;

import ar.edu.untref.robotica.motorfisico.Calculadora;
import ar.edu.untref.robotica.motorfisico.Rueda;
import ar.edu.untref.robotica.motorfisico.Sensor;

public class Robot {

	private Sensor sensorFront1;
	private Sensor sensorFront2;
	private Sensor sensor45Derecha;
	private Sensor sensor45Izquierda;
	private Sensor sensorAtras;
	private Rueda ruedaI;
	private Rueda ruedaD;
	private Calculadora calculadora = new Calculadora();

	private Escenario escenario;

	// En centimetros
	private double alto;
	private double ancho;

	public Robot(Escenario escenario, double alto, double ancho) {

		Configuracion configuracion = Configuracion.instancia();
		
		this.sensorFront1 = new Sensor
				(configuracion.getSensorFrontal1Angulo(), configuracion.getSensorFrontal1X(), configuracion.getSensorFrontal1Y());
		this.sensorFront2 = new Sensor
				(configuracion.getSensorFrontal2Angulo(), configuracion.getSensorFrontal2X(), configuracion.getSensorFrontal2Y());
		this.sensor45Derecha = new Sensor
				(configuracion.getSensorDerechoAngulo(), configuracion.getSensorDerechoX(), configuracion.getSensorDerechoY());
		this.sensor45Izquierda = new Sensor
				(configuracion.getSensorIzquierdoAngulo(), configuracion.getSensorIzquierdoX(), configuracion.getSensorIzquierdoY());
		this.sensorAtras = new Sensor
				(configuracion.getSensorAtrasAngulo(), configuracion.getSensorAtrasX(), configuracion.getSensorAtrasY());
		
		this.escenario = escenario;

		this.alto = alto;
		this.ancho = ancho;

		this.ruedaI = new Rueda();
		this.ruedaD = new Rueda();
	}

	public Rueda getRuedaI() {
		return ruedaI;
	}

	public Rueda getRuedaD() {
		return ruedaD;
	}

	public double getAlto() {
		// nunca es usado
		return alto;
	}

	public double getAncho() {
		// nunca es usado
		return ancho;
	}

	public double getDistanciaObstaculoAdelante(Escenario escenario) {
		return (this.sensorFront1.sensar(escenario) + this.sensorFront2
				.sensar(escenario)) / 2;
	}

	public double getDistanciaObstaculoAtras(Escenario escenario) {
		return this.sensorAtras.sensar(escenario);
	}

	public double getDistanciaObstaculo45Derecha(Escenario escenario) {
		return this.sensor45Derecha.sensar(escenario);
	}

	public double getDistanciaObstaculo45Izquierda(Escenario escenario) {
		return this.sensor45Izquierda.sensar(escenario);
	}

	public void avanzar() {
		this.ruedaI.acelerar(1);
		this.ruedaD.acelerar(1);
	}

	public void frenar() {
		this.ruedaI.acelerar(0);
		this.ruedaD.acelerar(0);
	}

	public void retroceder() {
		this.ruedaI.acelerar(-1);
		this.ruedaD.acelerar(-1);
	}

	public void aumentarAngulo() {
		this.escenario.aumentarAngulo(1);
	}

	public void disminuirAngulo() {
		this.escenario.disminuirAngulo(1);
	}
	
	public double getVelocidad(){
//		double d= calculadora.getCambioDeOrientacion(getRuedaI().getVelocidad(), getRuedaD().getVelocidad());
//		this.escenario.aumentarAngulo(d);
//		System.out.println("                              "+d);
		return (this.getRuedaI().getVelocidad() + this.getRuedaD().getVelocidad()) / 2;
	}

}